<!DOCTYPE html>
<html lang="en">
<head>
    <meta charset="utf-8">
    <title>src/equations/Equation.js - cannon</title>
    <link rel="stylesheet" href="http://yui.yahooapis.com/3.9.1/build/cssgrids/cssgrids-min.css">
    <link rel="stylesheet" href="../assets/vendor/prettify/prettify-min.css">
    <link rel="stylesheet" href="../assets/css/main.css" id="site_styles">
    <link rel="icon" href="../assets/favicon.ico">
    <script src="http://yui.yahooapis.com/combo?3.9.1/build/yui/yui-min.js"></script>
</head>
<body class="yui3-skin-sam">

<div id="doc">
    <div id="hd" class="yui3-g header">
        <div class="yui3-u-3-4">
                <h1><img src="../assets/css/logo.png" title="cannon" width="117" height="52"></h1>
        </div>
        <div class="yui3-u-1-4 version">
            <em>API Docs for: 0.6.1</em>
        </div>
    </div>
    <div id="bd" class="yui3-g">

        <div class="yui3-u-1-4">
            <div id="docs-sidebar" class="sidebar apidocs">
                <div id="api-list">
                    <h2 class="off-left">APIs</h2>
                    <div id="api-tabview" class="tabview">
                        <ul class="tabs">
                            <li><a href="#api-classes">Classes</a></li>
                            <li><a href="#api-modules">Modules</a></li>
                        </ul>
                
                        <div id="api-tabview-filter">
                            <input type="search" id="api-filter" placeholder="Type to filter APIs">
                        </div>
                
                        <div id="api-tabview-panel">
                            <ul id="api-classes" class="apis classes">
                                <li><a href="../classes/AABB.html">AABB</a></li>
                                <li><a href="../classes/ArrayCollisionMatrix.html">ArrayCollisionMatrix</a></li>
                                <li><a href="../classes/Body.html">Body</a></li>
                                <li><a href="../classes/Box.html">Box</a></li>
                                <li><a href="../classes/Broadphase.html">Broadphase</a></li>
                                <li><a href="../classes/ConeEquation.html">ConeEquation</a></li>
                                <li><a href="../classes/ConeTwistConstraint.html">ConeTwistConstraint</a></li>
                                <li><a href="../classes/Constraint.html">Constraint</a></li>
                                <li><a href="../classes/ContactEquation.html">ContactEquation</a></li>
                                <li><a href="../classes/ContactMaterial.html">ContactMaterial</a></li>
                                <li><a href="../classes/ConvexPolyhedron.html">ConvexPolyhedron</a></li>
                                <li><a href="../classes/Cylinder.html">Cylinder</a></li>
                                <li><a href="../classes/Demo.html">Demo</a></li>
                                <li><a href="../classes/DistanceConstraint.html">DistanceConstraint</a></li>
                                <li><a href="../classes/Equation.html">Equation</a></li>
                                <li><a href="../classes/EventTarget.html">EventTarget</a></li>
                                <li><a href="../classes/FrictionEquation.html">FrictionEquation</a></li>
                                <li><a href="../classes/GridBroadphase.html">GridBroadphase</a></li>
                                <li><a href="../classes/GSSolver.html">GSSolver</a></li>
                                <li><a href="../classes/Heightfield.html">Heightfield</a></li>
                                <li><a href="../classes/HingeConstraint.html">HingeConstraint</a></li>
                                <li><a href="../classes/JacobianElement.html">JacobianElement</a></li>
                                <li><a href="../classes/LockConstraint.html">LockConstraint</a></li>
                                <li><a href="../classes/Mat3.html">Mat3</a></li>
                                <li><a href="../classes/Material.html">Material</a></li>
                                <li><a href="../classes/NaiveBroadphase.html">NaiveBroadphase</a></li>
                                <li><a href="../classes/Narrowphase.html">Narrowphase</a></li>
                                <li><a href="../classes/ObjectCollisionMatrix.html">ObjectCollisionMatrix</a></li>
                                <li><a href="../classes/Octree.html">Octree</a></li>
                                <li><a href="../classes/OctreeNode.html">OctreeNode</a></li>
                                <li><a href="../classes/Particle.html">Particle</a></li>
                                <li><a href="../classes/Plane.html">Plane</a></li>
                                <li><a href="../classes/PointToPointConstraint.html">PointToPointConstraint</a></li>
                                <li><a href="../classes/Pool.html">Pool</a></li>
                                <li><a href="../classes/Quaternion.html">Quaternion</a></li>
                                <li><a href="../classes/Ray.html">Ray</a></li>
                                <li><a href="../classes/RaycastResult.html">RaycastResult</a></li>
                                <li><a href="../classes/RaycastVehicle.html">RaycastVehicle</a></li>
                                <li><a href="../classes/RigidVehicle.html">RigidVehicle</a></li>
                                <li><a href="../classes/RotationalEquation.html">RotationalEquation</a></li>
                                <li><a href="../classes/RotationalMotorEquation.html">RotationalMotorEquation</a></li>
                                <li><a href="../classes/SAPBroadphase.html">SAPBroadphase</a></li>
                                <li><a href="../classes/Shape.html">Shape</a></li>
                                <li><a href="../classes/Solver.html">Solver</a></li>
                                <li><a href="../classes/Sphere.html">Sphere</a></li>
                                <li><a href="../classes/SPHSystem.html">SPHSystem</a></li>
                                <li><a href="../classes/SplitSolver.html">SplitSolver</a></li>
                                <li><a href="../classes/Spring.html">Spring</a></li>
                                <li><a href="../classes/Transform.html">Transform</a></li>
                                <li><a href="../classes/Trimesh.html">Trimesh</a></li>
                                <li><a href="../classes/TupleDictionary.html">TupleDictionary</a></li>
                                <li><a href="../classes/Vec3.html">Vec3</a></li>
                                <li><a href="../classes/Vec3Pool.html">Vec3Pool</a></li>
                                <li><a href="../classes/WheelInfo.html">WheelInfo</a></li>
                                <li><a href="../classes/World.html">World</a></li>
                            </ul>
                
                            <ul id="api-modules" class="apis modules">
                            </ul>
                        </div>
                    </div>
                </div>
            </div>
        </div>
        <div class="yui3-u-3-4">
                <div id="api-options">
                    Show:
                    <label for="api-show-inherited">
                        <input type="checkbox" id="api-show-inherited" checked>
                        Inherited
                    </label>
            
                    <label for="api-show-protected">
                        <input type="checkbox" id="api-show-protected">
                        Protected
                    </label>
            
                    <label for="api-show-private">
                        <input type="checkbox" id="api-show-private">
                        Private
                    </label>
                    <label for="api-show-deprecated">
                        <input type="checkbox" id="api-show-deprecated">
                        Deprecated
                    </label>
            
                </div>
            
            <div class="apidocs">
                <div id="docs-main">
                    <div class="content">
<h1 class="file-heading">File: src/equations/Equation.js</h1>

<div class="file">
    <pre class="code prettyprint linenums">
module.exports = Equation;

var JacobianElement = require(&#x27;../math/JacobianElement&#x27;),
    Vec3 = require(&#x27;../math/Vec3&#x27;);

/**
 * Equation base class
 * @class Equation
 * @constructor
 * @author schteppe
 * @param {Body} bi
 * @param {Body} bj
 * @param {Number} minForce Minimum (read: negative max) force to be applied by the constraint.
 * @param {Number} maxForce Maximum (read: positive max) force to be applied by the constraint.
 */
function Equation(bi,bj,minForce,maxForce){
    this.id = Equation.id++;

    /**
     * @property {number} minForce
     */
    this.minForce = typeof(minForce)===&quot;undefined&quot; ? -1e6 : minForce;

    /**
     * @property {number} maxForce
     */
    this.maxForce = typeof(maxForce)===&quot;undefined&quot; ? 1e6 : maxForce;

    /**
     * @property bi
     * @type {Body}
     */
    this.bi = bi;

    /**
     * @property bj
     * @type {Body}
     */
    this.bj = bj;

    /**
     * SPOOK parameter
     * @property {number} a
     */
    this.a = 0.0;

    /**
     * SPOOK parameter
     * @property {number} b
     */
    this.b = 0.0;

    /**
     * SPOOK parameter
     * @property {number} eps
     */
    this.eps = 0.0;

    /**
     * @property {JacobianElement} jacobianElementA
     */
    this.jacobianElementA = new JacobianElement();

    /**
     * @property {JacobianElement} jacobianElementB
     */
    this.jacobianElementB = new JacobianElement();

    /**
     * @property {boolean} enabled
     * @default true
     */
    this.enabled = true;

    // Set typical spook params
    this.setSpookParams(1e7,4,1/60);
}
Equation.prototype.constructor = Equation;

Equation.id = 0;

/**
 * Recalculates a,b,eps.
 * @method setSpookParams
 */
Equation.prototype.setSpookParams = function(stiffness,relaxation,timeStep){
    var d = relaxation,
        k = stiffness,
        h = timeStep;
    this.a = 4.0 / (h * (1 + 4 * d));
    this.b = (4.0 * d) / (1 + 4 * d);
    this.eps = 4.0 / (h * h * k * (1 + 4 * d));
};

/**
 * Computes the RHS of the SPOOK equation
 * @method computeB
 * @return {Number}
 */
Equation.prototype.computeB = function(a,b,h){
    var GW = this.computeGW(),
        Gq = this.computeGq(),
        GiMf = this.computeGiMf();
    return - Gq * a - GW * b - GiMf*h;
};

/**
 * Computes G*q, where q are the generalized body coordinates
 * @method computeGq
 * @return {Number}
 */
Equation.prototype.computeGq = function(){
    var GA = this.jacobianElementA,
        GB = this.jacobianElementB,
        bi = this.bi,
        bj = this.bj,
        xi = bi.position,
        xj = bj.position;
    return GA.spatial.dot(xi) + GB.spatial.dot(xj);
};

var zero = new Vec3();

/**
 * Computes G*W, where W are the body velocities
 * @method computeGW
 * @return {Number}
 */
Equation.prototype.computeGW = function(){
    var GA = this.jacobianElementA,
        GB = this.jacobianElementB,
        bi = this.bi,
        bj = this.bj,
        vi = bi.velocity,
        vj = bj.velocity,
        wi = bi.angularVelocity || zero,
        wj = bj.angularVelocity || zero;
    return GA.multiplyVectors(vi,wi) + GB.multiplyVectors(vj,wj);
};


/**
 * Computes G*Wlambda, where W are the body velocities
 * @method computeGWlambda
 * @return {Number}
 */
Equation.prototype.computeGWlambda = function(){
    var GA = this.jacobianElementA,
        GB = this.jacobianElementB,
        bi = this.bi,
        bj = this.bj,
        vi = bi.vlambda,
        vj = bj.vlambda,
        wi = bi.wlambda || zero,
        wj = bj.wlambda || zero;
    return GA.multiplyVectors(vi,wi) + GB.multiplyVectors(vj,wj);
};

/**
 * Computes G*inv(M)*f, where M is the mass matrix with diagonal blocks for each body, and f are the forces on the bodies.
 * @method computeGiMf
 * @return {Number}
 */
var iMfi = new Vec3(),
    iMfj = new Vec3(),
    invIi_vmult_taui = new Vec3(),
    invIj_vmult_tauj = new Vec3();
Equation.prototype.computeGiMf = function(){
    var GA = this.jacobianElementA,
        GB = this.jacobianElementB,
        bi = this.bi,
        bj = this.bj,
        fi = bi.force,
        ti = bi.torque,
        fj = bj.force,
        tj = bj.torque,
        invMassi = bi.invMassSolve,
        invMassj = bj.invMassSolve;

    if(bi.invInertiaWorldSolve){ bi.invInertiaWorldSolve.vmult(ti,invIi_vmult_taui); }
    else { invIi_vmult_taui.set(0,0,0); }
    if(bj.invInertiaWorldSolve){ bj.invInertiaWorldSolve.vmult(tj,invIj_vmult_tauj); }
    else { invIj_vmult_tauj.set(0,0,0); }

    fi.mult(invMassi,iMfi);
    fj.mult(invMassj,iMfj);

    return GA.multiplyVectors(iMfi,invIi_vmult_taui) + GB.multiplyVectors(iMfj,invIj_vmult_tauj);
};

/**
 * Computes G*inv(M)*G&#x27;
 * @method computeGiMGt
 * @return {Number}
 */
var tmp = new Vec3();
Equation.prototype.computeGiMGt = function(){
    var GA = this.jacobianElementA,
        GB = this.jacobianElementB,
        bi = this.bi,
        bj = this.bj,
        invMassi = bi.invMassSolve,
        invMassj = bj.invMassSolve,
        invIi = bi.invInertiaWorldSolve,
        invIj = bj.invInertiaWorldSolve,
        result = invMassi + invMassj;

    if(invIi){
        invIi.vmult(GA.rotational,tmp);
        result += tmp.dot(GA.rotational);
    }

    if(invIj){
        invIj.vmult(GB.rotational,tmp);
        result += tmp.dot(GB.rotational);
    }

    return  result;
};

var addToWlambda_temp = new Vec3(),
    addToWlambda_Gi = new Vec3(),
    addToWlambda_Gj = new Vec3(),
    addToWlambda_ri = new Vec3(),
    addToWlambda_rj = new Vec3(),
    addToWlambda_Mdiag = new Vec3();

/**
 * Add constraint velocity to the bodies.
 * @method addToWlambda
 * @param {Number} deltalambda
 */
Equation.prototype.addToWlambda = function(deltalambda){
    var GA = this.jacobianElementA,
        GB = this.jacobianElementB,
        bi = this.bi,
        bj = this.bj,
        temp = addToWlambda_temp;

    // Add to linear velocity
    // v_lambda += inv(M) * delta_lamba * G
    GA.spatial.mult(bi.invMassSolve * deltalambda,temp);
    bi.vlambda.vadd(temp, bi.vlambda);

    GB.spatial.mult(bj.invMassSolve * deltalambda,temp);
    bj.vlambda.vadd(temp, bj.vlambda);

    // Add to angular velocity
    if(bi.invInertiaWorldSolve){
        bi.invInertiaWorldSolve.vmult(GA.rotational,temp);
        temp.mult(deltalambda,temp);
        bi.wlambda.vadd(temp,bi.wlambda);
    }

    if(bj.invInertiaWorldSolve){
        bj.invInertiaWorldSolve.vmult(GB.rotational,temp);
        temp.mult(deltalambda,temp);
        bj.wlambda.vadd(temp,bj.wlambda);
    }
};

/**
 * Compute the denominator part of the SPOOK equation: C = G*inv(M)*G&#x27; + eps
 * @method computeInvC
 * @param  {Number} eps
 * @return {Number}
 */
Equation.prototype.computeC = function(){
    return this.computeGiMGt() + this.eps;
};

    </pre>
</div>
                    </div>
                </div>
            </div>
        </div>
    </div>
</div>
<script src="../assets/vendor/prettify/prettify-min.js"></script>
<script>prettyPrint();</script>
<script src="../assets/js/yui-prettify.js"></script>
<script src="../assets/../api.js"></script>
<script src="../assets/js/api-filter.js"></script>
<script src="../assets/js/api-list.js"></script>
<script src="../assets/js/api-search.js"></script>
<script src="../assets/js/apidocs.js"></script>
</body>
</html>
